#include <stdlib.h>

#include "image_proc.h"

#include "cc3.h"

#define _DEBUG_R 0

// Definition d'une zone
#define SEG_MAX_ZONES 100
typedef struct{
 // le numero du seuil qui a detecte l'objet
 uint8_t classe;
 // position du coin superieur gauche de la zone
 uint16_t x_min;
 uint16_t y_min;
 // position du coin inferieur droit de la zone
 uint16_t x_max;
 uint16_t y_max;
 // Information de forme sur la zone: densite
 //float densite;
} zone;


#define MAX_CLASSE 2
int get_parameter(uint16_t Y ,uint16_t Cb,uint16_t Cr){
  if ((Y<102*SUBSIZE_SIZE)&&(Cr>=179*SUBSIZE_SIZE)&&(Cb<77*SUBSIZE_SIZE)) // ROUGE
    return 2;
  if ((Y>51*SUBSIZE_SIZE)&&(Y<=179*SUBSIZE_SIZE)&&(Cb<26*SUBSIZE_SIZE)&&(Cr>51*SUBSIZE_SIZE)&&(Cr<179*SUBSIZE_SIZE)) //JAUNE
    return 1;
  return 0; //0 is the unknow class
}


void compute_frm(int frm_nb){
  uint32_t x, y, k, pos;
  uint32_t size_x, size_y;
  uint32_t size_subs_x, size_subs_y;
  uint32_t pos_subs_x, pos_subs_y;

  uint32_t time, time2;
  int write_time;

  time = cc3_timer_get_current_ms();
  printf("Computing frame %d\n",frm_nb);

  size_x = cc3_g_pixbuf_frame.width;
  size_y = cc3_g_pixbuf_frame.height;
  if (size_x%SUBSIZE_X_RATIO>0)
    size_subs_x=(size_x/SUBSIZE_X_RATIO)+1;
  else
    size_subs_x=(size_x/SUBSIZE_X_RATIO);
  if (size_y%SUBSIZE_Y_RATIO>0)
    size_subs_y=(size_y/SUBSIZE_Y_RATIO)+1;
  else
    size_subs_y=(size_y/SUBSIZE_Y_RATIO);
  printf("Dims img sub X=%ld Y=%ld\n",size_subs_x,size_subs_y);


  uint16_t *img_subs=(uint16_t *)calloc(size_subs_x*size_subs_y*3,sizeof(uint16_t));
  uint8_t *row = cc3_malloc_rows(1);

  cc3_pixbuf_load ();

  // TEST: use MMC
  #if _DEBUG_R>0
  FILE *f;
  FILE *g;
  FILE *h;
  cc3_filesystem_init();
  cc3_timer_wait_ms(1000);
  f = fopen("c:/imgycbcr.txt", "w");
  g = fopen("c:/imgsubs.txt", "w");
  h = fopen("c:/result.txt", "w");
  cc3_timer_wait_ms(1000);
  printf("Files open. Pointers: f=%X g=%X\n", (int)f, (int)g);
  printf("Data pointers: img_subs=%X row=%X\n", (int)img_subs, (int)row);
  fprintf(f,"%ld %ld\n",size_x,size_y);
  fprintf(g,"%ld %ld\n",size_subs_x,size_subs_y);
  #endif
  // TEST: fin
  
  /* Parcourt de l'image et sous échantillonnage */
  for (y = 0; y < size_y; y++){
    cc3_pixbuf_read_rows(row, 1);
    for (x = 0; x < size_x; x++){
      pos_subs_x=x/SUBSIZE_X_RATIO;
      pos_subs_y=y/SUBSIZE_Y_RATIO;
      img_subs[3*(size_subs_x*pos_subs_y+pos_subs_x)]+=row[x*3U];
      img_subs[3*(size_subs_x*pos_subs_y+pos_subs_x)+1]+=row[x*3U+1];
      img_subs[3*(size_subs_x*pos_subs_y+pos_subs_x)+2]+=row[x*3U+2];

      // TEST: writefile
      #if _DEBUG_R>9
      fprintf(f,"%3u %3u %3u\n",row[x*3U],row[x*3U+1],row[x*3U+2]);
      #endif
      // TEST: fin
    }
  }
   
  /* extraction de parametres depuis l'image sous échantillonnées */
  uint8_t *img_param=calloc(size_subs_x*size_subs_y,sizeof(uint8_t));
  for (y = 0; y < size_subs_y; y++){
    for (x = 0; x < size_subs_x; x++){
      // Attention le mode de lecture de la camera est yCRcb et on bosse en yCBCr
      img_param[size_subs_x*y+x]=
                get_parameter(img_subs[3*(size_subs_x*y+x)  ],
                              img_subs[3*(size_subs_x*y+x)+2],
                              img_subs[3*(size_subs_x*y+x)+1]);
      
      // TEST: writefile
      #if _DEBUG_R>8
      img_subs[3*(size_subs_x*y+x)]  /=SUBSIZE_SIZE;
      img_subs[3*(size_subs_x*y+x)+1]/=SUBSIZE_SIZE;
      img_subs[3*(size_subs_x*y+x)+2]/=SUBSIZE_SIZE;
      fprintf(g,"%3u %3u %3u %3u\n",img_subs[3*(size_subs_x*y+x)]
                            ,img_subs[3* (size_subs_x*y+x)+1]
                            ,img_subs[3*(size_subs_x*y+x)+2]
                            ,img_param[size_subs_x*y+x]);
      #endif
      // TEST: fin
    }
  }

  // Détection de zones sur img_param
  uint8_t* linked=(uint8_t *)malloc(SEG_MAX_ZONES*sizeof(uint8_t));
  for( k=1; k<=MAX_CLASSE; k++ ) {
    //printf("Classe %ld\n",k);
    uint8_t* img_label=(uint8_t *)calloc(size_subs_x*size_subs_y,sizeof(uint8_t));

    // Initialisation des liens
    for( pos=0; pos<SEG_MAX_ZONES; pos++ ) {
      linked[pos]=pos;
    }

    int next_label=1;
    // Première passe
    for (y = 0; y < size_subs_y; y++){
      for (x = 0; x < size_subs_x; x++){
        //fprintf(h,"Img_param=%d k=%ld\n",img_param[size_subs_x*y+x],k);
        if (img_param[size_subs_x*y+x]==k){  //si on est sur une case active
          //fprintf(h,"Detection x=%ld y=%ld ",x,y);
          int last_case_label=0;
          if ((x>0)&&(img_label[size_subs_x*y+x-1]!=0)){  //si la case précedente est remplie, on prend le même label
            last_case_label=img_label[size_subs_x*y+x-1];
          }

          int last_line_label=0;
          if ((y>0)&&((img_label[(y-1)*size_subs_x+x-1]!=0)||(img_label[(y-1)*size_subs_x+x]!=0)||(img_label[(y-1)*size_subs_x+x+1]!=0))){  //si la ligne précedente est remplie, on prend le même label
            if (img_label[(y-1)*size_subs_x+x+1]!=0){
              last_line_label=img_label[(y-1)*size_subs_x+x+1];
            }else{if (img_label[(y-1)*size_subs_x+x]!=0){
              last_line_label=img_label[(y-1)*size_subs_x+x];
            }else{
              last_line_label=img_label[(y-1)*size_subs_x+x-1];
            }}
          }
          //fprintf(h,"last_line_label=%d last_case_label=%d\n",last_line_label,last_case_label);

          if (last_line_label!=0){
            img_label[y*size_subs_x+x]=last_line_label;
            if ((last_case_label!=last_line_label)&&(last_case_label!=0)){
              linked[last_case_label]=last_line_label;
            }
          }else{if (last_case_label!=0){
            img_label[y*size_subs_x+x]=last_case_label;
          }else{
            //fprintf(h,"Nouveau label next_label=%d\n", next_label);
            img_label[y*size_subs_x+x]=next_label;
            if (next_label<SEG_MAX_ZONES-1) next_label++;
          }}

        }
      }
    }
    
    for( pos=0; pos<SEG_MAX_ZONES; pos++ ) {
      if (linked[pos]!=pos){
        x=pos;
        while (linked[x]!=x) x=linked[x];
        linked[pos]=x;
      }
    }

    // Seconde passe
    zone* raw_zone=(zone *)calloc(SEG_MAX_ZONES,sizeof(zone));
    uint8_t* is_zone_valid=(uint8_t *)calloc(SEG_MAX_ZONES,sizeof(uint8_t));
    for (y = 0; y < size_subs_y; y++){
      for (x = 0; x < size_subs_x; x++){
        if (img_label[y*size_subs_x+x]!=0){
          int idx=linked[img_label[y*size_subs_x+x]];
          if (is_zone_valid[idx]==0){ // la zone n'est pas initialise
            raw_zone[idx].classe=k;
            raw_zone[idx].x_min=x;
            raw_zone[idx].y_min=y;
            raw_zone[idx].x_max=x;
            raw_zone[idx].y_max=y;
            is_zone_valid[idx]=1;
          }else{
            if (raw_zone[idx].x_min>x) raw_zone[idx].x_min=x;
            if (raw_zone[idx].x_max<x) raw_zone[idx].x_max=x;
            if (raw_zone[idx].y_min>y) raw_zone[idx].y_min=y;
            if (raw_zone[idx].y_max<y) raw_zone[idx].y_max=y;
          }
        }
      }
    }


    // Action sur les zones detectés
    for (x=0;x<SEG_MAX_ZONES;x++){
      if (is_zone_valid[x]==1){
        //printf("\n  --== ZONE %d ==--  \n",raw_zone[x].classe);
        //printf("   x_min=%d x_max=%d\n",raw_zone[x].x_min,raw_zone[x].x_max);
        //printf("   y_min=%d y_max=%d\n",raw_zone[x].y_min,raw_zone[x].y_max);
        printf("\n%d\n",raw_zone[x].classe);
        printf("%d %d\n",raw_zone[x].x_min,raw_zone[x].x_max);
        printf("%d %d\n",raw_zone[x].y_min,raw_zone[x].y_max);



      // TEST: writefile
      #if _DEBUG_R>8
        //fprintf(h,"\n  --== ZONE %d ==--  \n",raw_zone[x].classe);
        //fprintf(h,"   x_min=%d x_max=%d\n",raw_zone[x].x_min,raw_zone[x].x_max);
        //fprintf(h,"   y_min=%d y_max=%d\n",raw_zone[x].y_min,raw_zone[x].y_max);
        printf("\n%d\n",raw_zone[x].classe);
        printf("%d %d\n",raw_zone[x].x_min,raw_zone[x].x_max);
        printf("%d %d\n",raw_zone[x].y_min,raw_zone[x].y_max);

      #endif
      // TEST: fin
      }
    }
    free(is_zone_valid);
    free(raw_zone);
  }

  free(img_param);

  // TEST: closefile
  #if _DEBUG_R>0
  printf("Closing file: ");
  fclose (f);
  fclose (g);
  fclose (h);
  printf("ok\n");
  #endif
  // TEST: fin
 
  free(row);
  free(img_subs);

  time2 = cc3_timer_get_current_ms();
  write_time = time2 - time;
  printf("Computation time: %10d ms\n----------\n",write_time);

}


